#include <string>
#include <iostream>
#include <stdio.h>
#include <sstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "../includes/uEye/uEye.h"
#include "../header/UEyeOpenCV.h"
#include "../header/UEyeOpenCVException.h"
#include "../header/Settings.h"
#include "../header/UeyeCameraImpl.h"
using namespace std;
using namespace cv;

int main(int argc, char** argv) {
	string outputFileName;
	Mat orig;
	try {

		uint16_t height;
		uint16_t width;

		UeyeCameraImpl cam1 = UeyeCameraImpl();

		cam1.getImageFormat(&width, &height);

		cam1.openCamera();

		Mat cameraMatrix, distCoeffs, map1, map2, rMat, newCameraMatrix;
		Settings s;

		const string inputSettingsFile = "misc/config.xml";
		FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
		if (!fs.isOpened()) {
			cout << "Could not open the configuration file: \""
					<< inputSettingsFile << "\"" << endl;
			return -1;
		}

		fs["Settings"] >> s;

		fs.release();
		if (!s.goodInput) {
			cout << "Invalid input detected. Application stopping. " << endl;
			return -1;
		}

		FileStorage fs2(s.outputFileName, FileStorage::READ); // Read the settings
		if (!fs2.isOpened()) {
			cout << "Could not open the output file: \"" << s.outputFileName
					<< "\"" << endl;
			return -1;
		}
		fs2["Camera_Matrix"] >> cameraMatrix;
		fs2["Distortion_Coefficients"] >> distCoeffs;

		fs2.release();

		Mat orig(height, width, CV_8UC1);
		Mat undist(height, width, CV_8UC1);
		namedWindow("orig", 1);
		namedWindow("undist", 1);
		Size imageSize = orig.size();

		initUndistortRectifyMap(cameraMatrix, distCoeffs, rMat, newCameraMatrix,
				imageSize, CV_16SC2, map1, map2);

		while (true) {

			cam1.getFrame(orig);

			imshow("orig", orig);

//			if (s.flipVertical) {
//				flip(orig, orig, 0);
//			}
//			undistort(orig, undist, cameraMatrix, distCoeffs);
//
//
//			imshow("undist", undist);

			remap(orig, undist, map1, map2, CV_INTER_LINEAR, BORDER_CONSTANT,
					Scalar(0, 0, 0));

			imshow("undist", undist);

			if (waitKey(30) >= 0)
				break;
		}

	} catch (UeyeOpenCVException& e) {
		cout << e.what();
	}

	return 0;
}
;
